<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
<html xmlns="http://www.w3.org/1999/xhtml">
<head>
<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
<meta http-equiv="X-UA-Compatible" content="IE=9"/>
<meta name="generator" content="Doxygen 1.8.13"/>
<meta name="viewport" content="width=device-width, initial-scale=1"/>
<title>MRSL Motion Primitive Library: include/mpl_planner/env/env_map.h Source File</title>
<link href="tabs.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="jquery.js"></script>
<script type="text/javascript" src="dynsections.js"></script>
<link href="search/search.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="search/searchdata.js"></script>
<script type="text/javascript" src="search/search.js"></script>
<link href="doxygen.css" rel="stylesheet" type="text/css" />
</head>
<body>
<div id="top"><!-- do not remove this div, it is closed by doxygen! -->
<div id="titlearea">
<table cellspacing="0" cellpadding="0">
 <tbody>
 <tr style="height: 56px;">
  <td id="projectalign" style="padding-left: 0.5em;">
   <div id="projectname">MRSL Motion Primitive Library
   &#160;<span id="projectnumber">1.2</span>
   </div>
   <div id="projectbrief">A motion primitive library for generating trajectory for mobile robots</div>
  </td>
 </tr>
 </tbody>
</table>
</div>
<!-- end header part -->
<!-- Generated by Doxygen 1.8.13 -->
<script type="text/javascript">
var searchBox = new SearchBox("searchBox", "search",false,'Search');
</script>
<script type="text/javascript" src="menudata.js"></script>
<script type="text/javascript" src="menu.js"></script>
<script type="text/javascript">
$(function() {
  initMenu('',true,false,'search.php','Search');
  $(document).ready(function() { init_search(); });
});
</script>
<div id="main-nav"></div>
<!-- window showing the filter options -->
<div id="MSearchSelectWindow"
     onmouseover="return searchBox.OnSearchSelectShow()"
     onmouseout="return searchBox.OnSearchSelectHide()"
     onkeydown="return searchBox.OnSearchSelectKey(event)">
</div>

<!-- iframe showing the search results (closed by default) -->
<div id="MSearchResultsWindow">
<iframe src="javascript:void(0)" frameborder="0" 
        name="MSearchResults" id="MSearchResults">
</iframe>
</div>

<div id="nav-path" class="navpath">
  <ul>
<li class="navelem"><a class="el" href="dir_d44c64559bbebec7f509842c48db8b23.html">include</a></li><li class="navelem"><a class="el" href="dir_27679d0403171f1d79ee57f30a72bf4e.html">mpl_planner</a></li><li class="navelem"><a class="el" href="dir_bb30c8566c0dccf7838beceebdf748de.html">env</a></li>  </ul>
</div>
</div><!-- top -->
<div class="header">
  <div class="headertitle">
<div class="title">env_map.h</div>  </div>
</div><!--header-->
<div class="contents">
<a href="env__map_8h.html">Go to the documentation of this file.</a><div class="fragment"><div class="line"><a name="l00001"></a><span class="lineno">    1</span>&#160;</div><div class="line"><a name="l00006"></a><span class="lineno">    6</span>&#160;<span class="preprocessor">#ifndef MPL_ENV_MAP_H</span></div><div class="line"><a name="l00007"></a><span class="lineno">    7</span>&#160;<span class="preprocessor">#define MPL_ENV_MAP_H</span></div><div class="line"><a name="l00008"></a><span class="lineno">    8</span>&#160;<span class="preprocessor">#include &lt;<a class="code" href="map__util_8h.html">mpl_collision/map_util.h</a>&gt;</span></div><div class="line"><a name="l00009"></a><span class="lineno">    9</span>&#160;<span class="preprocessor">#include &lt;<a class="code" href="env__base_8h.html">mpl_planner/common/env_base.h</a>&gt;</span></div><div class="line"><a name="l00010"></a><span class="lineno">   10</span>&#160;</div><div class="line"><a name="l00011"></a><span class="lineno">   11</span>&#160;<span class="preprocessor">#include &lt;memory&gt;</span></div><div class="line"><a name="l00012"></a><span class="lineno">   12</span>&#160;<span class="preprocessor">#include &lt;unordered_map&gt;</span></div><div class="line"><a name="l00013"></a><span class="lineno">   13</span>&#160;</div><div class="line"><a name="l00014"></a><span class="lineno">   14</span>&#160;<span class="keyword">namespace </span><a class="code" href="namespaceMPL.html">MPL</a> {</div><div class="line"><a name="l00018"></a><span class="lineno">   18</span>&#160;<span class="keyword">template</span> &lt;<span class="keywordtype">int</span> Dim&gt;</div><div class="line"><a name="l00019"></a><span class="lineno"><a class="line" href="classMPL_1_1env__map.html">   19</a></span>&#160;<span class="keyword">class </span><a class="code" href="classMPL_1_1env__map.html">env_map</a> : <span class="keyword">public</span> <a class="code" href="classMPL_1_1env__base.html">env_base</a>&lt;Dim&gt; {</div><div class="line"><a name="l00020"></a><span class="lineno">   20</span>&#160; <span class="keyword">public</span>:</div><div class="line"><a name="l00022"></a><span class="lineno"><a class="line" href="classMPL_1_1env__map.html#a1c34f1f4e9c822470d3a15c88f3164fc">   22</a></span>&#160;  <a class="code" href="classMPL_1_1env__map.html#a1c34f1f4e9c822470d3a15c88f3164fc">env_map</a>(std::shared_ptr&lt;<a class="code" href="classMPL_1_1MapUtil.html">MapUtil&lt;Dim&gt;</a>&gt; map_util) : <a class="code" href="classMPL_1_1env__map.html#a0d59fd81da8205001281395b80a4c6fa">map_util_</a>(map_util) {}</div><div class="line"><a name="l00023"></a><span class="lineno">   23</span>&#160;</div><div class="line"><a name="l00025"></a><span class="lineno"><a class="line" href="classMPL_1_1env__map.html#a40c301e16f4413100e8600b3931be7ad">   25</a></span>&#160;  <span class="keywordtype">bool</span> <a class="code" href="classMPL_1_1env__map.html#a40c301e16f4413100e8600b3931be7ad">is_goal</a>(<span class="keyword">const</span> <a class="code" href="structWaypoint.html">Waypoint&lt;Dim&gt;</a> &amp;state)<span class="keyword"> const </span>{</div><div class="line"><a name="l00026"></a><span class="lineno">   26</span>&#160;    <span class="keywordtype">bool</span> goaled =</div><div class="line"><a name="l00027"></a><span class="lineno">   27</span>&#160;        (state.<a class="code" href="structWaypoint.html#af63de420a521f105eea4ce098a51ad7e">pos</a> - this-&gt;<a class="code" href="classMPL_1_1env__base.html#a8e6706c18fa58da08174f7c0c8209312">goal_node_</a>.pos).<span class="keyword">template</span> lpNorm&lt;Eigen::Infinity&gt;() &lt;=</div><div class="line"><a name="l00028"></a><span class="lineno">   28</span>&#160;        this-&gt;<a class="code" href="classMPL_1_1env__base.html#ac85f1da0ec8736ba5c379bbdb6634fe2">tol_pos_</a>;</div><div class="line"><a name="l00029"></a><span class="lineno">   29</span>&#160;</div><div class="line"><a name="l00030"></a><span class="lineno">   30</span>&#160;    <span class="keywordflow">if</span> (goaled &amp;&amp; this-&gt;<a class="code" href="classMPL_1_1env__base.html#a2a6216a2e0a6fcd1bf2942b145daa889">tol_vel_</a> &gt;= 0)</div><div class="line"><a name="l00031"></a><span class="lineno">   31</span>&#160;      goaled = (state.<a class="code" href="structWaypoint.html#a91393e0d8d73afb9df512710e56f02a5">vel</a> - this-&gt;<a class="code" href="classMPL_1_1env__base.html#a8e6706c18fa58da08174f7c0c8209312">goal_node_</a>.vel)</div><div class="line"><a name="l00032"></a><span class="lineno">   32</span>&#160;                   .<span class="keyword">template</span> lpNorm&lt;Eigen::Infinity&gt;() &lt;= this-&gt;<a class="code" href="classMPL_1_1env__base.html#a2a6216a2e0a6fcd1bf2942b145daa889">tol_vel_</a>;</div><div class="line"><a name="l00033"></a><span class="lineno">   33</span>&#160;    <span class="keywordflow">if</span> (goaled &amp;&amp; this-&gt;<a class="code" href="classMPL_1_1env__base.html#ac49a422f3e1b826123a12389024b9c50">tol_acc_</a> &gt;= 0)</div><div class="line"><a name="l00034"></a><span class="lineno">   34</span>&#160;      goaled = (state.<a class="code" href="structWaypoint.html#a648217ed387d4e226bec8ecb05bfefe8">acc</a> - this-&gt;<a class="code" href="classMPL_1_1env__base.html#a8e6706c18fa58da08174f7c0c8209312">goal_node_</a>.acc)</div><div class="line"><a name="l00035"></a><span class="lineno">   35</span>&#160;                   .<span class="keyword">template</span> lpNorm&lt;Eigen::Infinity&gt;() &lt;= this-&gt;<a class="code" href="classMPL_1_1env__base.html#ac49a422f3e1b826123a12389024b9c50">tol_acc_</a>;</div><div class="line"><a name="l00036"></a><span class="lineno">   36</span>&#160;    <span class="keywordflow">if</span> (goaled &amp;&amp; this-&gt;<a class="code" href="classMPL_1_1env__base.html#aee3b399510cbd3dc9690d65f0c099673">tol_yaw_</a> &gt;= 0)</div><div class="line"><a name="l00037"></a><span class="lineno">   37</span>&#160;      goaled = std::abs(state.<a class="code" href="structWaypoint.html#ad5c2f2b8f6dba96bb93faccde844e9a8">yaw</a> - this-&gt;goal_node_.yaw) &lt;= this-&gt;<a class="code" href="classMPL_1_1env__base.html#aee3b399510cbd3dc9690d65f0c099673">tol_yaw_</a>;</div><div class="line"><a name="l00038"></a><span class="lineno">   38</span>&#160;    <span class="keywordflow">if</span> (goaled) {</div><div class="line"><a name="l00039"></a><span class="lineno">   39</span>&#160;      <span class="keyword">auto</span> pns = <a class="code" href="classMPL_1_1env__map.html#a0d59fd81da8205001281395b80a4c6fa">map_util_</a>-&gt;rayTrace(state.<a class="code" href="structWaypoint.html#af63de420a521f105eea4ce098a51ad7e">pos</a>, this-&gt;goal_node_.pos);</div><div class="line"><a name="l00040"></a><span class="lineno">   40</span>&#160;      <span class="keywordflow">for</span> (<span class="keyword">const</span> <span class="keyword">auto</span> &amp;it : pns) {</div><div class="line"><a name="l00041"></a><span class="lineno">   41</span>&#160;        <span class="keywordflow">if</span> (<a class="code" href="classMPL_1_1env__map.html#a0d59fd81da8205001281395b80a4c6fa">map_util_</a>-&gt;isOccupied(it)) <span class="keywordflow">return</span> <span class="keyword">false</span>;</div><div class="line"><a name="l00042"></a><span class="lineno">   42</span>&#160;      }</div><div class="line"><a name="l00043"></a><span class="lineno">   43</span>&#160;    }</div><div class="line"><a name="l00044"></a><span class="lineno">   44</span>&#160;    <span class="keywordflow">return</span> goaled;</div><div class="line"><a name="l00045"></a><span class="lineno">   45</span>&#160;  }</div><div class="line"><a name="l00046"></a><span class="lineno">   46</span>&#160;</div><div class="line"><a name="l00048"></a><span class="lineno"><a class="line" href="classMPL_1_1env__map.html#a257df36534163eef0292854d38477a6b">   48</a></span>&#160;  <span class="keywordtype">bool</span> <a class="code" href="classMPL_1_1env__map.html#a257df36534163eef0292854d38477a6b">is_free</a>(<span class="keyword">const</span> <a class="code" href="data__type_8h.html#a3a0c45655a5e009e56634ccde0c5c575">Vecf&lt;Dim&gt;</a> &amp;pt)<span class="keyword"> const </span>{</div><div class="line"><a name="l00049"></a><span class="lineno">   49</span>&#160;    <span class="keyword">const</span> <span class="keyword">auto</span> pn = <a class="code" href="classMPL_1_1env__map.html#a0d59fd81da8205001281395b80a4c6fa">map_util_</a>-&gt;floatToInt(pt);</div><div class="line"><a name="l00050"></a><span class="lineno">   50</span>&#160;    <span class="keywordflow">return</span> <a class="code" href="classMPL_1_1env__map.html#a0d59fd81da8205001281395b80a4c6fa">map_util_</a>-&gt;isFree(pn);</div><div class="line"><a name="l00051"></a><span class="lineno">   51</span>&#160;  }</div><div class="line"><a name="l00052"></a><span class="lineno">   52</span>&#160;</div><div class="line"><a name="l00060"></a><span class="lineno"><a class="line" href="classMPL_1_1env__map.html#a7229298e89baa8b1acb1ed1affda30a4">   60</a></span>&#160;  <span class="keywordtype">bool</span> <a class="code" href="classMPL_1_1env__map.html#a7229298e89baa8b1acb1ed1affda30a4">is_free</a>(<span class="keyword">const</span> <a class="code" href="classPrimitive.html">Primitive&lt;Dim&gt;</a> &amp;pr)<span class="keyword"> const </span>{</div><div class="line"><a name="l00061"></a><span class="lineno">   61</span>&#160;    <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> max_v = 0;</div><div class="line"><a name="l00062"></a><span class="lineno">   62</span>&#160;    <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i = 0; i &lt; Dim; i++) {</div><div class="line"><a name="l00063"></a><span class="lineno">   63</span>&#160;      <span class="keywordflow">if</span> (pr.<a class="code" href="classPrimitive.html#a6c369ebe499544a832c88b6fc4ada0e4">max_vel</a>(i) &gt; max_v) max_v = pr.<a class="code" href="classPrimitive.html#a6c369ebe499544a832c88b6fc4ada0e4">max_vel</a>(i);</div><div class="line"><a name="l00064"></a><span class="lineno">   64</span>&#160;    }</div><div class="line"><a name="l00065"></a><span class="lineno">   65</span>&#160;    <span class="keywordtype">int</span> n = std::ceil(max_v * pr.<a class="code" href="classPrimitive.html#a908ba5793fc0520602352b59358a34ea">t</a>() / <a class="code" href="classMPL_1_1env__map.html#a0d59fd81da8205001281395b80a4c6fa">map_util_</a>-&gt;getRes());</div><div class="line"><a name="l00066"></a><span class="lineno">   66</span>&#160;    <a class="code" href="data__type_8h.html#a6b9210d48a0a541940af71827aad1b41">vec_E&lt;Waypoint&lt;Dim&gt;</a>&gt; pts = pr.<a class="code" href="classPrimitive.html#ad46e6378e08cd0f2d829787d8c44b93d">sample</a>(n);</div><div class="line"><a name="l00067"></a><span class="lineno">   67</span>&#160;    <span class="keywordflow">for</span> (<span class="keyword">const</span> <span class="keyword">auto</span> &amp;pt : pts) {</div><div class="line"><a name="l00068"></a><span class="lineno">   68</span>&#160;      <a class="code" href="data__type_8h.html#ac208833a6aa7ec29f39eb5ae142aaede">Veci&lt;Dim&gt;</a> pn = <a class="code" href="classMPL_1_1env__map.html#a0d59fd81da8205001281395b80a4c6fa">map_util_</a>-&gt;floatToInt(pt.pos);</div><div class="line"><a name="l00069"></a><span class="lineno">   69</span>&#160;      <span class="keywordflow">if</span> (<a class="code" href="classMPL_1_1env__map.html#a0d59fd81da8205001281395b80a4c6fa">map_util_</a>-&gt;isOccupied(pn) || <a class="code" href="classMPL_1_1env__map.html#a0d59fd81da8205001281395b80a4c6fa">map_util_</a>-&gt;isOutside(pn)) <span class="keywordflow">return</span> <span class="keyword">false</span>;</div><div class="line"><a name="l00070"></a><span class="lineno">   70</span>&#160;      <span class="keywordflow">if</span> (!this-&gt;<a class="code" href="classMPL_1_1env__base.html#a8873f64d0e619a9945fc3be17b88c227">search_region_</a>.empty() &amp;&amp;</div><div class="line"><a name="l00071"></a><span class="lineno">   71</span>&#160;          !this-&gt;<a class="code" href="classMPL_1_1env__base.html#a8873f64d0e619a9945fc3be17b88c227">search_region_</a>[<a class="code" href="classMPL_1_1env__map.html#a0d59fd81da8205001281395b80a4c6fa">map_util_</a>-&gt;getIndex(pn)])</div><div class="line"><a name="l00072"></a><span class="lineno">   72</span>&#160;        <span class="keywordflow">return</span> <span class="keyword">false</span>;</div><div class="line"><a name="l00073"></a><span class="lineno">   73</span>&#160;    }</div><div class="line"><a name="l00074"></a><span class="lineno">   74</span>&#160;</div><div class="line"><a name="l00075"></a><span class="lineno">   75</span>&#160;    <span class="keywordflow">return</span> <span class="keyword">true</span>;</div><div class="line"><a name="l00076"></a><span class="lineno">   76</span>&#160;  }</div><div class="line"><a name="l00077"></a><span class="lineno">   77</span>&#160;</div><div class="line"><a name="l00090"></a><span class="lineno"><a class="line" href="classMPL_1_1env__map.html#a84ab1c20552e01476e40cc5f116d4d73">   90</a></span>&#160;  <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> <a class="code" href="classMPL_1_1env__map.html#a84ab1c20552e01476e40cc5f116d4d73">traverse_primitive</a>(<span class="keyword">const</span> <a class="code" href="classPrimitive.html">Primitive&lt;Dim&gt;</a> &amp;pr)<span class="keyword"> const </span>{</div><div class="line"><a name="l00091"></a><span class="lineno">   91</span>&#160;    <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> max_v = 0;</div><div class="line"><a name="l00092"></a><span class="lineno">   92</span>&#160;    <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i = 0; i &lt; Dim; i++) {</div><div class="line"><a name="l00093"></a><span class="lineno">   93</span>&#160;      <span class="keywordflow">if</span> (pr.<a class="code" href="classPrimitive.html#a6c369ebe499544a832c88b6fc4ada0e4">max_vel</a>(i) &gt; max_v) max_v = pr.<a class="code" href="classPrimitive.html#a6c369ebe499544a832c88b6fc4ada0e4">max_vel</a>(i);</div><div class="line"><a name="l00094"></a><span class="lineno">   94</span>&#160;    }</div><div class="line"><a name="l00095"></a><span class="lineno">   95</span>&#160;    <span class="keywordtype">int</span> n = std::max(5, (<span class="keywordtype">int</span>)std::ceil(max_v * pr.<a class="code" href="classPrimitive.html#a908ba5793fc0520602352b59358a34ea">t</a>() / <a class="code" href="classMPL_1_1env__map.html#a0d59fd81da8205001281395b80a4c6fa">map_util_</a>-&gt;getRes()));</div><div class="line"><a name="l00096"></a><span class="lineno">   96</span>&#160;    <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> c = 0;</div><div class="line"><a name="l00097"></a><span class="lineno">   97</span>&#160;</div><div class="line"><a name="l00098"></a><span class="lineno">   98</span>&#160;    <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> dt = pr.<a class="code" href="classPrimitive.html#a908ba5793fc0520602352b59358a34ea">t</a>() / n;</div><div class="line"><a name="l00099"></a><span class="lineno">   99</span>&#160;    <span class="keywordflow">for</span> (<a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> t = 0; t &lt; pr.<a class="code" href="classPrimitive.html#a908ba5793fc0520602352b59358a34ea">t</a>(); t += dt) {</div><div class="line"><a name="l00100"></a><span class="lineno">  100</span>&#160;      <span class="keyword">const</span> <span class="keyword">auto</span> pt = pr.<a class="code" href="classPrimitive.html#a2654dd3b648c5e3ee7839344a8d9e730">evaluate</a>(t);</div><div class="line"><a name="l00101"></a><span class="lineno">  101</span>&#160;      <span class="keyword">const</span> <a class="code" href="data__type_8h.html#ac208833a6aa7ec29f39eb5ae142aaede">Veci&lt;Dim&gt;</a> pn = <a class="code" href="classMPL_1_1env__map.html#a0d59fd81da8205001281395b80a4c6fa">map_util_</a>-&gt;floatToInt(pt.pos);</div><div class="line"><a name="l00102"></a><span class="lineno">  102</span>&#160;      <span class="keyword">const</span> <span class="keywordtype">int</span> idx = <a class="code" href="classMPL_1_1env__map.html#a0d59fd81da8205001281395b80a4c6fa">map_util_</a>-&gt;getIndex(pn);</div><div class="line"><a name="l00103"></a><span class="lineno">  103</span>&#160;</div><div class="line"><a name="l00104"></a><span class="lineno">  104</span>&#160;      <span class="keywordflow">if</span> (<a class="code" href="classMPL_1_1env__map.html#a0d59fd81da8205001281395b80a4c6fa">map_util_</a>-&gt;isOutside(pn) ||</div><div class="line"><a name="l00105"></a><span class="lineno">  105</span>&#160;          (!this-&gt;<a class="code" href="classMPL_1_1env__base.html#a8873f64d0e619a9945fc3be17b88c227">search_region_</a>.empty() &amp;&amp; !this-&gt;<a class="code" href="classMPL_1_1env__base.html#a8873f64d0e619a9945fc3be17b88c227">search_region_</a>[idx]))</div><div class="line"><a name="l00106"></a><span class="lineno">  106</span>&#160;        <span class="keywordflow">return</span> std::numeric_limits&lt;decimal_t&gt;::infinity();</div><div class="line"><a name="l00107"></a><span class="lineno">  107</span>&#160;      <span class="comment">/*</span></div><div class="line"><a name="l00108"></a><span class="lineno">  108</span>&#160;<span class="comment">      decimal_t v_value = gradient_map_[idx].dot(pt.vel);</span></div><div class="line"><a name="l00109"></a><span class="lineno">  109</span>&#160;<span class="comment">      if(v_value &gt; 0)</span></div><div class="line"><a name="l00110"></a><span class="lineno">  110</span>&#160;<span class="comment">        v_value = 0;</span></div><div class="line"><a name="l00111"></a><span class="lineno">  111</span>&#160;<span class="comment">      v_value = -v_value;</span></div><div class="line"><a name="l00112"></a><span class="lineno">  112</span>&#160;<span class="comment">      */</span></div><div class="line"><a name="l00113"></a><span class="lineno">  113</span>&#160;      <span class="keywordflow">if</span> (!<a class="code" href="classMPL_1_1env__map.html#ae596cf73659ff74a03723acba510616d">potential_map_</a>.empty()) {</div><div class="line"><a name="l00114"></a><span class="lineno">  114</span>&#160;        <span class="keywordflow">if</span> (<a class="code" href="classMPL_1_1env__map.html#ae596cf73659ff74a03723acba510616d">potential_map_</a>[idx] &lt; 100 &amp;&amp; <a class="code" href="classMPL_1_1env__map.html#ae596cf73659ff74a03723acba510616d">potential_map_</a>[idx] &gt; 0) {</div><div class="line"><a name="l00115"></a><span class="lineno">  115</span>&#160;          c += dt * (<a class="code" href="classMPL_1_1env__map.html#a3475ee37b51c1289a49726794b5b4451">potential_weight_</a> * <a class="code" href="classMPL_1_1env__map.html#ae596cf73659ff74a03723acba510616d">potential_map_</a>[idx] +</div><div class="line"><a name="l00116"></a><span class="lineno">  116</span>&#160;                     <a class="code" href="classMPL_1_1env__map.html#a5e41e4f10d5970bd03d512c7becce099">gradient_weight_</a> * pt.vel.norm());</div><div class="line"><a name="l00117"></a><span class="lineno">  117</span>&#160;        } <span class="keywordflow">else</span> <span class="keywordflow">if</span> (<a class="code" href="classMPL_1_1env__map.html#ae596cf73659ff74a03723acba510616d">potential_map_</a>[idx] &gt;= 100)</div><div class="line"><a name="l00118"></a><span class="lineno">  118</span>&#160;          <span class="keywordflow">return</span> std::numeric_limits&lt;decimal_t&gt;::infinity();</div><div class="line"><a name="l00119"></a><span class="lineno">  119</span>&#160;      } <span class="keywordflow">else</span> <span class="keywordflow">if</span> (<a class="code" href="classMPL_1_1env__map.html#a0d59fd81da8205001281395b80a4c6fa">map_util_</a>-&gt;isOccupied(pn))</div><div class="line"><a name="l00120"></a><span class="lineno">  120</span>&#160;        <span class="keywordflow">return</span> std::numeric_limits&lt;decimal_t&gt;::infinity();</div><div class="line"><a name="l00121"></a><span class="lineno">  121</span>&#160;      <span class="keywordflow">if</span> (this-&gt;<a class="code" href="classMPL_1_1env__base.html#ad47343816c81f60e207cd424a957c491">wyaw_</a> &gt; 0 &amp;&amp; pt.use_yaw) {</div><div class="line"><a name="l00122"></a><span class="lineno">  122</span>&#160;        <span class="keyword">const</span> <span class="keyword">auto</span> v = pt.vel.template topRows&lt;2&gt;();</div><div class="line"><a name="l00123"></a><span class="lineno">  123</span>&#160;        <span class="keywordflow">if</span> (v.norm() &gt; 1e-5) {  <span class="comment">// if v is not zero</span></div><div class="line"><a name="l00124"></a><span class="lineno">  124</span>&#160;          <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> v_value =</div><div class="line"><a name="l00125"></a><span class="lineno">  125</span>&#160;              1 - v.normalized().dot(<a class="code" href="data__type_8h.html#a135c596de9b80bec15985876ebd5036e">Vec2f</a>(cos(pt.yaw), sin(pt.yaw)));</div><div class="line"><a name="l00126"></a><span class="lineno">  126</span>&#160;          c += this-&gt;<a class="code" href="classMPL_1_1env__base.html#ad47343816c81f60e207cd424a957c491">wyaw_</a> * v_value * dt;</div><div class="line"><a name="l00127"></a><span class="lineno">  127</span>&#160;        }</div><div class="line"><a name="l00128"></a><span class="lineno">  128</span>&#160;      }</div><div class="line"><a name="l00129"></a><span class="lineno">  129</span>&#160;    }</div><div class="line"><a name="l00130"></a><span class="lineno">  130</span>&#160;</div><div class="line"><a name="l00131"></a><span class="lineno">  131</span>&#160;    <span class="keywordflow">return</span> c;</div><div class="line"><a name="l00132"></a><span class="lineno">  132</span>&#160;  }</div><div class="line"><a name="l00133"></a><span class="lineno">  133</span>&#160;</div><div class="line"><a name="l00147"></a><span class="lineno"><a class="line" href="classMPL_1_1env__map.html#ad508bed13e2c697cb14a597615424d9f">  147</a></span>&#160;  <span class="keywordtype">void</span> <a class="code" href="classMPL_1_1env__map.html#ad508bed13e2c697cb14a597615424d9f">get_succ</a>(<span class="keyword">const</span> <a class="code" href="structWaypoint.html">Waypoint&lt;Dim&gt;</a> &amp;curr, <a class="code" href="data__type_8h.html#a6b9210d48a0a541940af71827aad1b41">vec_E</a>&lt;<a class="code" href="structWaypoint.html">Waypoint&lt;Dim&gt;</a>&gt; &amp;succ,</div><div class="line"><a name="l00148"></a><span class="lineno">  148</span>&#160;                std::vector&lt;decimal_t&gt; &amp;succ_cost,</div><div class="line"><a name="l00149"></a><span class="lineno">  149</span>&#160;                std::vector&lt;int&gt; &amp;action_idx)<span class="keyword"> const </span>{</div><div class="line"><a name="l00150"></a><span class="lineno">  150</span>&#160;    succ.clear();</div><div class="line"><a name="l00151"></a><span class="lineno">  151</span>&#160;    succ_cost.clear();</div><div class="line"><a name="l00152"></a><span class="lineno">  152</span>&#160;    action_idx.clear();</div><div class="line"><a name="l00153"></a><span class="lineno">  153</span>&#160;</div><div class="line"><a name="l00154"></a><span class="lineno">  154</span>&#160;    this-&gt;<a class="code" href="classMPL_1_1env__base.html#a407c61fa8bfdefc3012546f0c412acd3">expanded_nodes_</a>.push_back(curr.<a class="code" href="structWaypoint.html#af63de420a521f105eea4ce098a51ad7e">pos</a>);</div><div class="line"><a name="l00155"></a><span class="lineno">  155</span>&#160;    <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i = 0; i &lt; this-&gt;<a class="code" href="classMPL_1_1env__base.html#a632147ec3e706f2bf10fb196c9d0a38c">U_</a>.size(); i++) {</div><div class="line"><a name="l00156"></a><span class="lineno">  156</span>&#160;      <a class="code" href="classPrimitive.html">Primitive&lt;Dim&gt;</a> pr(curr, this-&gt;<a class="code" href="classMPL_1_1env__base.html#a632147ec3e706f2bf10fb196c9d0a38c">U_</a>[i], this-&gt;<a class="code" href="classMPL_1_1env__base.html#a75662e4867f05ecc21cd1232d2126a11">dt_</a>);</div><div class="line"><a name="l00157"></a><span class="lineno">  157</span>&#160;      <a class="code" href="structWaypoint.html">Waypoint&lt;Dim&gt;</a> tn = pr.<a class="code" href="classPrimitive.html#a2654dd3b648c5e3ee7839344a8d9e730">evaluate</a>(this-&gt;<a class="code" href="classMPL_1_1env__base.html#a75662e4867f05ecc21cd1232d2126a11">dt_</a>);</div><div class="line"><a name="l00158"></a><span class="lineno">  158</span>&#160;      <span class="keywordflow">if</span> (tn == curr || !<a class="code" href="primitive_8h.html#a4e7cc1831110e0cd23fa969236c8b9f5">validate_primitive</a>(pr, this-&gt;<a class="code" href="classMPL_1_1env__base.html#adcd1fa1f64b3b836228d9a79f91dff0e">v_max_</a>, this-&gt;<a class="code" href="classMPL_1_1env__base.html#a7be6859be90f54ef175482d0aa6020c9">a_max_</a>,</div><div class="line"><a name="l00159"></a><span class="lineno">  159</span>&#160;                                            this-&gt;<a class="code" href="classMPL_1_1env__base.html#a6a67a6409ed46cd0c04f095a18996178">j_max_</a>, this-&gt;<a class="code" href="classMPL_1_1env__base.html#a6d1787397e20de82495fb9c381c14980">yaw_max_</a>))</div><div class="line"><a name="l00160"></a><span class="lineno">  160</span>&#160;        <span class="keywordflow">continue</span>;</div><div class="line"><a name="l00161"></a><span class="lineno">  161</span>&#160;      tn.<a class="code" href="structWaypoint.html#a9661067e17763fffb65d99cabaff802a">t</a> = curr.<a class="code" href="structWaypoint.html#a9661067e17763fffb65d99cabaff802a">t</a> + this-&gt;<a class="code" href="classMPL_1_1env__base.html#a75662e4867f05ecc21cd1232d2126a11">dt_</a>;</div><div class="line"><a name="l00162"></a><span class="lineno">  162</span>&#160;      succ.push_back(tn);</div><div class="line"><a name="l00163"></a><span class="lineno">  163</span>&#160;      <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> cost = curr.<a class="code" href="structWaypoint.html#af63de420a521f105eea4ce098a51ad7e">pos</a> == tn.<a class="code" href="structWaypoint.html#af63de420a521f105eea4ce098a51ad7e">pos</a> ? 0 : <a class="code" href="classMPL_1_1env__map.html#a84ab1c20552e01476e40cc5f116d4d73">traverse_primitive</a>(pr);</div><div class="line"><a name="l00164"></a><span class="lineno">  164</span>&#160;      <span class="keywordflow">if</span> (!std::isinf(cost)) {</div><div class="line"><a name="l00165"></a><span class="lineno">  165</span>&#160;        cost += this-&gt;calculate_intrinsic_cost(pr);</div><div class="line"><a name="l00166"></a><span class="lineno">  166</span>&#160;        this-&gt;<a class="code" href="classMPL_1_1env__base.html#a1d96af4d1aa7e1a15924316fae89174e">expanded_edges_</a>.push_back(pr);</div><div class="line"><a name="l00167"></a><span class="lineno">  167</span>&#160;      }</div><div class="line"><a name="l00168"></a><span class="lineno">  168</span>&#160;</div><div class="line"><a name="l00169"></a><span class="lineno">  169</span>&#160;      succ_cost.push_back(cost);</div><div class="line"><a name="l00170"></a><span class="lineno">  170</span>&#160;      action_idx.push_back(i);</div><div class="line"><a name="l00171"></a><span class="lineno">  171</span>&#160;    }</div><div class="line"><a name="l00172"></a><span class="lineno">  172</span>&#160;  }</div><div class="line"><a name="l00173"></a><span class="lineno">  173</span>&#160;</div><div class="line"><a name="l00175"></a><span class="lineno"><a class="line" href="classMPL_1_1env__map.html#a37c314a52d6bba0fa9675099d40b322c">  175</a></span>&#160;  <span class="keywordtype">void</span> <a class="code" href="classMPL_1_1env__map.html#a37c314a52d6bba0fa9675099d40b322c">set_gradient_map</a>(<span class="keyword">const</span> <a class="code" href="data__type_8h.html#a6b9210d48a0a541940af71827aad1b41">vec_E</a>&lt;<a class="code" href="data__type_8h.html#a3a0c45655a5e009e56634ccde0c5c575">Vecf&lt;Dim&gt;</a>&gt; &amp;map) { <a class="code" href="classMPL_1_1env__map.html#a1255eaee11eb80ce0e7f3a53f1f9b94e">gradient_map_</a> = map; }</div><div class="line"><a name="l00176"></a><span class="lineno">  176</span>&#160;</div><div class="line"><a name="l00178"></a><span class="lineno"><a class="line" href="classMPL_1_1env__map.html#a27101292af5b56676fca7374e1c1f5f1">  178</a></span>&#160;  <span class="keywordtype">void</span> <a class="code" href="classMPL_1_1env__map.html#a27101292af5b56676fca7374e1c1f5f1">set_gradient_weight</a>(<a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> w) { <a class="code" href="classMPL_1_1env__map.html#a5e41e4f10d5970bd03d512c7becce099">gradient_weight_</a> = w; }</div><div class="line"><a name="l00179"></a><span class="lineno">  179</span>&#160;</div><div class="line"><a name="l00181"></a><span class="lineno"><a class="line" href="classMPL_1_1env__map.html#a061bd8a136a679ee196455c8170269bc">  181</a></span>&#160;  <span class="keywordtype">void</span> <a class="code" href="classMPL_1_1env__map.html#a061bd8a136a679ee196455c8170269bc">set_potential_map</a>(<span class="keyword">const</span> std::vector&lt;int8_t&gt; &amp;map) {</div><div class="line"><a name="l00182"></a><span class="lineno">  182</span>&#160;    <a class="code" href="classMPL_1_1env__map.html#ae596cf73659ff74a03723acba510616d">potential_map_</a> = map;</div><div class="line"><a name="l00183"></a><span class="lineno">  183</span>&#160;  }</div><div class="line"><a name="l00184"></a><span class="lineno">  184</span>&#160;</div><div class="line"><a name="l00186"></a><span class="lineno"><a class="line" href="classMPL_1_1env__map.html#a4d832ec079d5ffe034aca016a9fa6227">  186</a></span>&#160;  <span class="keywordtype">void</span> <a class="code" href="classMPL_1_1env__map.html#a4d832ec079d5ffe034aca016a9fa6227">set_potential_weight</a>(<a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> w) { <a class="code" href="classMPL_1_1env__map.html#a3475ee37b51c1289a49726794b5b4451">potential_weight_</a> = w; }</div><div class="line"><a name="l00187"></a><span class="lineno">  187</span>&#160;</div><div class="line"><a name="l00189"></a><span class="lineno"><a class="line" href="classMPL_1_1env__map.html#a2d7be70921dce29559e33f3e59fc8c5a">  189</a></span>&#160;  <span class="keywordtype">void</span> <a class="code" href="classMPL_1_1env__map.html#a2d7be70921dce29559e33f3e59fc8c5a">set_prior_trajectory</a>(<span class="keyword">const</span> <a class="code" href="classTrajectory.html">Trajectory&lt;Dim&gt;</a> &amp;traj) {</div><div class="line"><a name="l00190"></a><span class="lineno">  190</span>&#160;    this-&gt;<a class="code" href="classMPL_1_1env__base.html#ad12eb1d6268bebc2542a875a3278551a">prior_traj_</a>.clear();</div><div class="line"><a name="l00191"></a><span class="lineno">  191</span>&#160;    <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> total_time = traj.<a class="code" href="classTrajectory.html#a93f4d5f86feb5f56e5fae7fd02311e9c">getTotalTime</a>();</div><div class="line"><a name="l00192"></a><span class="lineno">  192</span>&#160;    <span class="keyword">const</span> <span class="keywordtype">int</span> n = std::ceil(this-&gt;<a class="code" href="classMPL_1_1env__base.html#adcd1fa1f64b3b836228d9a79f91dff0e">v_max_</a> * total_time / <a class="code" href="classMPL_1_1env__map.html#a0d59fd81da8205001281395b80a4c6fa">map_util_</a>-&gt;getRes());</div><div class="line"><a name="l00193"></a><span class="lineno">  193</span>&#160;    <span class="keyword">const</span> <span class="keyword">auto</span> pts = traj.<a class="code" href="classTrajectory.html#a25ec384d286281c81480dee1b2f7bffe">sample</a>(n);</div><div class="line"><a name="l00194"></a><span class="lineno">  194</span>&#160;</div><div class="line"><a name="l00195"></a><span class="lineno">  195</span>&#160;    std::vector&lt;decimal_t&gt; costs;</div><div class="line"><a name="l00196"></a><span class="lineno">  196</span>&#160;    <span class="keywordflow">for</span> (<a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> t = 0; t &lt; total_time; t += this-&gt;<a class="code" href="classMPL_1_1env__base.html#a75662e4867f05ecc21cd1232d2126a11">dt_</a>) {</div><div class="line"><a name="l00197"></a><span class="lineno">  197</span>&#160;      <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> potential_cost = 0;</div><div class="line"><a name="l00198"></a><span class="lineno">  198</span>&#160;      <span class="keywordflow">if</span> (!<a class="code" href="classMPL_1_1env__map.html#ae596cf73659ff74a03723acba510616d">potential_map_</a>.empty()) {</div><div class="line"><a name="l00199"></a><span class="lineno">  199</span>&#160;        <span class="keywordtype">int</span> prev_idx = -1;</div><div class="line"><a name="l00200"></a><span class="lineno">  200</span>&#160;        <span class="keywordflow">for</span> (<span class="keyword">const</span> <span class="keyword">auto</span> &amp;pt : pts) {</div><div class="line"><a name="l00201"></a><span class="lineno">  201</span>&#160;          <span class="keywordflow">if</span> (pt.t &gt;= t) <span class="keywordflow">break</span>;</div><div class="line"><a name="l00202"></a><span class="lineno">  202</span>&#160;          <span class="keyword">const</span> <a class="code" href="data__type_8h.html#ac208833a6aa7ec29f39eb5ae142aaede">Veci&lt;Dim&gt;</a> pn = <a class="code" href="classMPL_1_1env__map.html#a0d59fd81da8205001281395b80a4c6fa">map_util_</a>-&gt;floatToInt(pt.pos);</div><div class="line"><a name="l00203"></a><span class="lineno">  203</span>&#160;          <span class="keyword">const</span> <span class="keywordtype">int</span> idx = <a class="code" href="classMPL_1_1env__map.html#a0d59fd81da8205001281395b80a4c6fa">map_util_</a>-&gt;getIndex(pn);</div><div class="line"><a name="l00204"></a><span class="lineno">  204</span>&#160;          <span class="keywordflow">if</span> (prev_idx == idx)</div><div class="line"><a name="l00205"></a><span class="lineno">  205</span>&#160;            <span class="keywordflow">continue</span>;</div><div class="line"><a name="l00206"></a><span class="lineno">  206</span>&#160;          <span class="keywordflow">else</span></div><div class="line"><a name="l00207"></a><span class="lineno">  207</span>&#160;            prev_idx = idx;</div><div class="line"><a name="l00208"></a><span class="lineno">  208</span>&#160;          potential_cost += <a class="code" href="classMPL_1_1env__map.html#a3475ee37b51c1289a49726794b5b4451">potential_weight_</a> * <a class="code" href="classMPL_1_1env__map.html#ae596cf73659ff74a03723acba510616d">potential_map_</a>[idx] +</div><div class="line"><a name="l00209"></a><span class="lineno">  209</span>&#160;                            <a class="code" href="classMPL_1_1env__map.html#a5e41e4f10d5970bd03d512c7becce099">gradient_weight_</a> * pt.vel.norm();</div><div class="line"><a name="l00210"></a><span class="lineno">  210</span>&#160;        }</div><div class="line"><a name="l00211"></a><span class="lineno">  211</span>&#160;      }</div><div class="line"><a name="l00212"></a><span class="lineno">  212</span>&#160;      costs.push_back(this-&gt;<a class="code" href="classMPL_1_1env__base.html#a5c6a2e729131e36e72b51266d887ae8f">w_</a> * t + potential_cost);</div><div class="line"><a name="l00213"></a><span class="lineno">  213</span>&#160;      printf(<span class="stringliteral">&quot;t: %.2f, cost: %f\n&quot;</span>, t, costs.back());</div><div class="line"><a name="l00214"></a><span class="lineno">  214</span>&#160;    }</div><div class="line"><a name="l00215"></a><span class="lineno">  215</span>&#160;</div><div class="line"><a name="l00216"></a><span class="lineno">  216</span>&#160;    <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> total_cost = <a class="code" href="classMPL_1_1env__map.html#affcb3f4534d828b9d2751292cef5cf24">traverse_trajectory</a>(traj) + this-&gt;<a class="code" href="classMPL_1_1env__base.html#a5c6a2e729131e36e72b51266d887ae8f">w_</a> * total_time;</div><div class="line"><a name="l00217"></a><span class="lineno">  217</span>&#160;    printf(<span class="stringliteral">&quot;total cost: %f\n&quot;</span>, total_cost);</div><div class="line"><a name="l00218"></a><span class="lineno">  218</span>&#160;</div><div class="line"><a name="l00219"></a><span class="lineno">  219</span>&#160;    <span class="keywordflow">for</span> (<a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> t = 0; t &lt; total_time; t += this-&gt;<a class="code" href="classMPL_1_1env__base.html#a75662e4867f05ecc21cd1232d2126a11">dt_</a>) {</div><div class="line"><a name="l00220"></a><span class="lineno">  220</span>&#160;      <span class="keywordtype">int</span> <span class="keywordtype">id</span> = t / this-&gt;<a class="code" href="classMPL_1_1env__base.html#a75662e4867f05ecc21cd1232d2126a11">dt_</a>;</div><div class="line"><a name="l00221"></a><span class="lineno">  221</span>&#160;      this-&gt;<a class="code" href="classMPL_1_1env__base.html#ad12eb1d6268bebc2542a875a3278551a">prior_traj_</a>.push_back(</div><div class="line"><a name="l00222"></a><span class="lineno">  222</span>&#160;          std::make_pair(traj.<a class="code" href="classTrajectory.html#aec62c9d9b1a35202df2072ea3efcef09">evaluate</a>(t), total_cost - costs[id]));</div><div class="line"><a name="l00223"></a><span class="lineno">  223</span>&#160;    }</div><div class="line"><a name="l00224"></a><span class="lineno">  224</span>&#160;</div><div class="line"><a name="l00225"></a><span class="lineno">  225</span>&#160;    this-&gt;<a class="code" href="classMPL_1_1env__base.html#a8e6706c18fa58da08174f7c0c8209312">goal_node_</a> = traj.<a class="code" href="classTrajectory.html#aec62c9d9b1a35202df2072ea3efcef09">evaluate</a>(total_time);  <span class="comment">// set goal</span></div><div class="line"><a name="l00226"></a><span class="lineno">  226</span>&#160;  }</div><div class="line"><a name="l00227"></a><span class="lineno">  227</span>&#160;</div><div class="line"><a name="l00229"></a><span class="lineno"><a class="line" href="classMPL_1_1env__map.html#affcb3f4534d828b9d2751292cef5cf24">  229</a></span>&#160;  <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> <a class="code" href="classMPL_1_1env__map.html#affcb3f4534d828b9d2751292cef5cf24">traverse_trajectory</a>(<span class="keyword">const</span> <a class="code" href="classTrajectory.html">Trajectory&lt;Dim&gt;</a> &amp;traj)<span class="keyword"> const </span>{</div><div class="line"><a name="l00230"></a><span class="lineno">  230</span>&#160;    <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> total_time = traj.<a class="code" href="classTrajectory.html#a93f4d5f86feb5f56e5fae7fd02311e9c">getTotalTime</a>();</div><div class="line"><a name="l00231"></a><span class="lineno">  231</span>&#160;    <span class="keywordtype">int</span> n = std::ceil(this-&gt;<a class="code" href="classMPL_1_1env__base.html#adcd1fa1f64b3b836228d9a79f91dff0e">v_max_</a> * total_time / <a class="code" href="classMPL_1_1env__map.html#a0d59fd81da8205001281395b80a4c6fa">map_util_</a>-&gt;getRes());</div><div class="line"><a name="l00232"></a><span class="lineno">  232</span>&#160;    <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> c = 0;</div><div class="line"><a name="l00233"></a><span class="lineno">  233</span>&#160;    <span class="keyword">const</span> <span class="keyword">auto</span> pts = traj.<a class="code" href="classTrajectory.html#a25ec384d286281c81480dee1b2f7bffe">sample</a>(n);</div><div class="line"><a name="l00234"></a><span class="lineno">  234</span>&#160;    <span class="keywordtype">int</span> prev_idx = -1;</div><div class="line"><a name="l00235"></a><span class="lineno">  235</span>&#160;    <span class="keywordflow">for</span> (<span class="keyword">const</span> <span class="keyword">auto</span> &amp;pt : pts) {</div><div class="line"><a name="l00236"></a><span class="lineno">  236</span>&#160;      <span class="keyword">const</span> <a class="code" href="data__type_8h.html#ac208833a6aa7ec29f39eb5ae142aaede">Veci&lt;Dim&gt;</a> pn = <a class="code" href="classMPL_1_1env__map.html#a0d59fd81da8205001281395b80a4c6fa">map_util_</a>-&gt;floatToInt(pt.pos);</div><div class="line"><a name="l00237"></a><span class="lineno">  237</span>&#160;      <span class="keyword">const</span> <span class="keywordtype">int</span> idx = <a class="code" href="classMPL_1_1env__map.html#a0d59fd81da8205001281395b80a4c6fa">map_util_</a>-&gt;getIndex(pn);</div><div class="line"><a name="l00238"></a><span class="lineno">  238</span>&#160;      <span class="keywordflow">if</span> (prev_idx == idx)</div><div class="line"><a name="l00239"></a><span class="lineno">  239</span>&#160;        <span class="keywordflow">continue</span>;</div><div class="line"><a name="l00240"></a><span class="lineno">  240</span>&#160;      <span class="keywordflow">else</span></div><div class="line"><a name="l00241"></a><span class="lineno">  241</span>&#160;        prev_idx = idx;</div><div class="line"><a name="l00242"></a><span class="lineno">  242</span>&#160;      <span class="keywordflow">if</span> (<a class="code" href="classMPL_1_1env__map.html#a0d59fd81da8205001281395b80a4c6fa">map_util_</a>-&gt;isOutside(pn))</div><div class="line"><a name="l00243"></a><span class="lineno">  243</span>&#160;        <span class="keywordflow">return</span> std::numeric_limits&lt;decimal_t&gt;::infinity();</div><div class="line"><a name="l00244"></a><span class="lineno">  244</span>&#160;      <span class="keywordflow">if</span> (!<a class="code" href="classMPL_1_1env__map.html#ae596cf73659ff74a03723acba510616d">potential_map_</a>.empty()) {</div><div class="line"><a name="l00245"></a><span class="lineno">  245</span>&#160;        <span class="keywordflow">if</span> (<a class="code" href="classMPL_1_1env__map.html#ae596cf73659ff74a03723acba510616d">potential_map_</a>[idx] &lt; 100 &amp;&amp; <a class="code" href="classMPL_1_1env__map.html#ae596cf73659ff74a03723acba510616d">potential_map_</a>[idx] &gt; 0) {</div><div class="line"><a name="l00246"></a><span class="lineno">  246</span>&#160;          c += <a class="code" href="classMPL_1_1env__map.html#a3475ee37b51c1289a49726794b5b4451">potential_weight_</a> * <a class="code" href="classMPL_1_1env__map.html#ae596cf73659ff74a03723acba510616d">potential_map_</a>[idx] +</div><div class="line"><a name="l00247"></a><span class="lineno">  247</span>&#160;               <a class="code" href="classMPL_1_1env__map.html#a5e41e4f10d5970bd03d512c7becce099">gradient_weight_</a> * pt.vel.norm();</div><div class="line"><a name="l00248"></a><span class="lineno">  248</span>&#160;        } <span class="keywordflow">else</span> <span class="keywordflow">if</span> (<a class="code" href="classMPL_1_1env__map.html#ae596cf73659ff74a03723acba510616d">potential_map_</a>[idx] &gt;= 100)</div><div class="line"><a name="l00249"></a><span class="lineno">  249</span>&#160;          <span class="keywordflow">return</span> std::numeric_limits&lt;decimal_t&gt;::infinity();</div><div class="line"><a name="l00250"></a><span class="lineno">  250</span>&#160;      } <span class="keywordflow">else</span> <span class="keywordflow">if</span> (<a class="code" href="classMPL_1_1env__map.html#a0d59fd81da8205001281395b80a4c6fa">map_util_</a>-&gt;isOccupied(pn))</div><div class="line"><a name="l00251"></a><span class="lineno">  251</span>&#160;        <span class="keywordflow">return</span> std::numeric_limits&lt;decimal_t&gt;::infinity();</div><div class="line"><a name="l00252"></a><span class="lineno">  252</span>&#160;    }</div><div class="line"><a name="l00253"></a><span class="lineno">  253</span>&#160;</div><div class="line"><a name="l00254"></a><span class="lineno">  254</span>&#160;    <span class="keywordflow">return</span> c;</div><div class="line"><a name="l00255"></a><span class="lineno">  255</span>&#160;  }</div><div class="line"><a name="l00256"></a><span class="lineno">  256</span>&#160;</div><div class="line"><a name="l00258"></a><span class="lineno"><a class="line" href="classMPL_1_1env__map.html#ae21503c9a6461602c4e7e09db66b8111">  258</a></span>&#160;  <span class="keywordtype">void</span> <a class="code" href="classMPL_1_1env__map.html#ae21503c9a6461602c4e7e09db66b8111">info</a>() {</div><div class="line"><a name="l00259"></a><span class="lineno">  259</span>&#160;    printf(<span class="stringliteral">&quot;++++++++++++++++++++ env_map ++++++++++++++++++\n&quot;</span>);</div><div class="line"><a name="l00260"></a><span class="lineno">  260</span>&#160;    printf(<span class="stringliteral">&quot;+                  w: %.2f               +\n&quot;</span>, this-&gt;<a class="code" href="classMPL_1_1env__base.html#a5c6a2e729131e36e72b51266d887ae8f">w_</a>);</div><div class="line"><a name="l00261"></a><span class="lineno">  261</span>&#160;    printf(<span class="stringliteral">&quot;+               wyaw: %.2f               +\n&quot;</span>, this-&gt;<a class="code" href="classMPL_1_1env__base.html#ad47343816c81f60e207cd424a957c491">wyaw_</a>);</div><div class="line"><a name="l00262"></a><span class="lineno">  262</span>&#160;    printf(<span class="stringliteral">&quot;+                 dt: %.2f               +\n&quot;</span>, this-&gt;<a class="code" href="classMPL_1_1env__base.html#a75662e4867f05ecc21cd1232d2126a11">dt_</a>);</div><div class="line"><a name="l00263"></a><span class="lineno">  263</span>&#160;    printf(<span class="stringliteral">&quot;+              t_max: %.2f               +\n&quot;</span>, this-&gt;<a class="code" href="classMPL_1_1env__base.html#ad592f0117a0ef0ac15225855ae62c4cc">t_max_</a>);</div><div class="line"><a name="l00264"></a><span class="lineno">  264</span>&#160;    printf(<span class="stringliteral">&quot;+              v_max: %.2f               +\n&quot;</span>, this-&gt;<a class="code" href="classMPL_1_1env__base.html#adcd1fa1f64b3b836228d9a79f91dff0e">v_max_</a>);</div><div class="line"><a name="l00265"></a><span class="lineno">  265</span>&#160;    printf(<span class="stringliteral">&quot;+              a_max: %.2f               +\n&quot;</span>, this-&gt;<a class="code" href="classMPL_1_1env__base.html#a7be6859be90f54ef175482d0aa6020c9">a_max_</a>);</div><div class="line"><a name="l00266"></a><span class="lineno">  266</span>&#160;    printf(<span class="stringliteral">&quot;+              j_max: %.2f               +\n&quot;</span>, this-&gt;<a class="code" href="classMPL_1_1env__base.html#a6a67a6409ed46cd0c04f095a18996178">j_max_</a>);</div><div class="line"><a name="l00267"></a><span class="lineno">  267</span>&#160;    printf(<span class="stringliteral">&quot;+            yaw_max: %.2f               +\n&quot;</span>, this-&gt;<a class="code" href="classMPL_1_1env__base.html#a6d1787397e20de82495fb9c381c14980">yaw_max_</a>);</div><div class="line"><a name="l00268"></a><span class="lineno">  268</span>&#160;    printf(<span class="stringliteral">&quot;+              U num: %zu                +\n&quot;</span>, this-&gt;<a class="code" href="classMPL_1_1env__base.html#a632147ec3e706f2bf10fb196c9d0a38c">U_</a>.size());</div><div class="line"><a name="l00269"></a><span class="lineno">  269</span>&#160;    printf(<span class="stringliteral">&quot;+            tol_pos: %.2f               +\n&quot;</span>, this-&gt;<a class="code" href="classMPL_1_1env__base.html#ac85f1da0ec8736ba5c379bbdb6634fe2">tol_pos_</a>);</div><div class="line"><a name="l00270"></a><span class="lineno">  270</span>&#160;    printf(<span class="stringliteral">&quot;+            tol_vel: %.2f               +\n&quot;</span>, this-&gt;<a class="code" href="classMPL_1_1env__base.html#a2a6216a2e0a6fcd1bf2942b145daa889">tol_vel_</a>);</div><div class="line"><a name="l00271"></a><span class="lineno">  271</span>&#160;    printf(<span class="stringliteral">&quot;+            tol_acc: %.2f               +\n&quot;</span>, this-&gt;<a class="code" href="classMPL_1_1env__base.html#ac49a422f3e1b826123a12389024b9c50">tol_acc_</a>);</div><div class="line"><a name="l00272"></a><span class="lineno">  272</span>&#160;    printf(<span class="stringliteral">&quot;+            tol_yaw: %.2f               +\n&quot;</span>, this-&gt;<a class="code" href="classMPL_1_1env__base.html#aee3b399510cbd3dc9690d65f0c099673">tol_yaw_</a>);</div><div class="line"><a name="l00273"></a><span class="lineno">  273</span>&#160;    printf(<span class="stringliteral">&quot;+heur_ignore_dynamics: %d                 +\n&quot;</span>,</div><div class="line"><a name="l00274"></a><span class="lineno">  274</span>&#160;           this-&gt;<a class="code" href="classMPL_1_1env__base.html#a12ccadddac2cf7cf643ce4af8982054c">heur_ignore_dynamics_</a>);</div><div class="line"><a name="l00275"></a><span class="lineno">  275</span>&#160;    <span class="keywordflow">if</span> (!<a class="code" href="classMPL_1_1env__map.html#ae596cf73659ff74a03723acba510616d">potential_map_</a>.empty())</div><div class="line"><a name="l00276"></a><span class="lineno">  276</span>&#160;      printf(<span class="stringliteral">&quot;+    potential_weight: %.2f                 +\n&quot;</span>,</div><div class="line"><a name="l00277"></a><span class="lineno">  277</span>&#160;             <a class="code" href="classMPL_1_1env__map.html#a3475ee37b51c1289a49726794b5b4451">potential_weight_</a>);</div><div class="line"><a name="l00278"></a><span class="lineno">  278</span>&#160;    <span class="keywordflow">if</span> (!<a class="code" href="classMPL_1_1env__map.html#a1255eaee11eb80ce0e7f3a53f1f9b94e">gradient_map_</a>.empty())</div><div class="line"><a name="l00279"></a><span class="lineno">  279</span>&#160;      printf(<span class="stringliteral">&quot;+     gradient_weight: %.2f                 +\n&quot;</span>,</div><div class="line"><a name="l00280"></a><span class="lineno">  280</span>&#160;             <a class="code" href="classMPL_1_1env__map.html#a5e41e4f10d5970bd03d512c7becce099">gradient_weight_</a>);</div><div class="line"><a name="l00281"></a><span class="lineno">  281</span>&#160;    <span class="keywordflow">if</span> (!this-&gt;<a class="code" href="classMPL_1_1env__base.html#ad12eb1d6268bebc2542a875a3278551a">prior_traj_</a>.empty())</div><div class="line"><a name="l00282"></a><span class="lineno">  282</span>&#160;      printf(<span class="stringliteral">&quot;+     use_prior_traj: true                 +\n&quot;</span>);</div><div class="line"><a name="l00283"></a><span class="lineno">  283</span>&#160;    printf(<span class="stringliteral">&quot;++++++++++++++++++++ env_map ++++++++++++++++++\n&quot;</span>);</div><div class="line"><a name="l00284"></a><span class="lineno">  284</span>&#160;  }</div><div class="line"><a name="l00285"></a><span class="lineno">  285</span>&#160;</div><div class="line"><a name="l00286"></a><span class="lineno">  286</span>&#160; <span class="keyword">protected</span>:</div><div class="line"><a name="l00288"></a><span class="lineno"><a class="line" href="classMPL_1_1env__map.html#a0d59fd81da8205001281395b80a4c6fa">  288</a></span>&#160;  std::shared_ptr&lt;MapUtil&lt;Dim&gt;&gt; <a class="code" href="classMPL_1_1env__map.html#a0d59fd81da8205001281395b80a4c6fa">map_util_</a>;</div><div class="line"><a name="l00290"></a><span class="lineno"><a class="line" href="classMPL_1_1env__map.html#ae596cf73659ff74a03723acba510616d">  290</a></span>&#160;  std::vector&lt;int8_t&gt; <a class="code" href="classMPL_1_1env__map.html#ae596cf73659ff74a03723acba510616d">potential_map_</a>;</div><div class="line"><a name="l00292"></a><span class="lineno"><a class="line" href="classMPL_1_1env__map.html#a1255eaee11eb80ce0e7f3a53f1f9b94e">  292</a></span>&#160;  <a class="code" href="data__type_8h.html#a6b9210d48a0a541940af71827aad1b41">vec_E&lt;Vecf&lt;Dim&gt;</a>&gt; <a class="code" href="classMPL_1_1env__map.html#a1255eaee11eb80ce0e7f3a53f1f9b94e">gradient_map_</a>;</div><div class="line"><a name="l00294"></a><span class="lineno"><a class="line" href="classMPL_1_1env__map.html#a3475ee37b51c1289a49726794b5b4451">  294</a></span>&#160;  <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> <a class="code" href="classMPL_1_1env__map.html#a3475ee37b51c1289a49726794b5b4451">potential_weight_</a>{0.1};</div><div class="line"><a name="l00296"></a><span class="lineno"><a class="line" href="classMPL_1_1env__map.html#a5e41e4f10d5970bd03d512c7becce099">  296</a></span>&#160;  <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> <a class="code" href="classMPL_1_1env__map.html#a5e41e4f10d5970bd03d512c7becce099">gradient_weight_</a>{0.0};</div><div class="line"><a name="l00297"></a><span class="lineno">  297</span>&#160;};</div><div class="line"><a name="l00298"></a><span class="lineno">  298</span>&#160;}  <span class="comment">// namespace MPL</span></div><div class="line"><a name="l00299"></a><span class="lineno">  299</span>&#160;</div><div class="line"><a name="l00300"></a><span class="lineno">  300</span>&#160;<span class="preprocessor">#endif</span></div><div class="ttc" id="classMPL_1_1env__base_html_ad12eb1d6268bebc2542a875a3278551a"><div class="ttname"><a href="classMPL_1_1env__base.html#ad12eb1d6268bebc2542a875a3278551a">MPL::env_base::prior_traj_</a></div><div class="ttdeci">vec_E&lt; std::pair&lt; Waypoint&lt; Dim &gt;, decimal_t &gt; &gt; prior_traj_</div><div class="ttdoc">Prior trajectory. </div><div class="ttdef"><b>Definition:</b> env_base.h:398</div></div>
<div class="ttc" id="classTrajectory_html_aec62c9d9b1a35202df2072ea3efcef09"><div class="ttname"><a href="classTrajectory.html#aec62c9d9b1a35202df2072ea3efcef09">Trajectory::evaluate</a></div><div class="ttdeci">Waypoint&lt; Dim &gt; evaluate(decimal_t time) const</div><div class="ttdoc">Evaluate Waypoint at time . </div><div class="ttdef"><b>Definition:</b> trajectory.h:67</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_a8873f64d0e619a9945fc3be17b88c227"><div class="ttname"><a href="classMPL_1_1env__base.html#a8873f64d0e619a9945fc3be17b88c227">MPL::env_base::search_region_</a></div><div class="ttdeci">std::vector&lt; bool &gt; search_region_</div><div class="ttdoc">Valid search region (tunnel constraint) </div><div class="ttdef"><b>Definition:</b> env_base.h:400</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_a2a6216a2e0a6fcd1bf2942b145daa889"><div class="ttname"><a href="classMPL_1_1env__base.html#a2a6216a2e0a6fcd1bf2942b145daa889">MPL::env_base::tol_vel_</a></div><div class="ttdeci">decimal_t tol_vel_</div><div class="ttdoc">tolerance of velocity for goal region, 0 means no tolerance </div><div class="ttdef"><b>Definition:</b> env_base.h:376</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_a1d96af4d1aa7e1a15924316fae89174e"><div class="ttname"><a href="classMPL_1_1env__base.html#a1d96af4d1aa7e1a15924316fae89174e">MPL::env_base::expanded_edges_</a></div><div class="ttdeci">vec_E&lt; Primitive&lt; Dim &gt; &gt; expanded_edges_</div><div class="ttdoc">expanded edges for debug </div><div class="ttdef"><b>Definition:</b> env_base.h:404</div></div>
<div class="ttc" id="namespaceMPL_html"><div class="ttname"><a href="namespaceMPL.html">MPL</a></div><div class="ttdef"><b>Definition:</b> map_util.h:12</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_ad47343816c81f60e207cd424a957c491"><div class="ttname"><a href="classMPL_1_1env__base.html#ad47343816c81f60e207cd424a957c491">MPL::env_base::wyaw_</a></div><div class="ttdeci">decimal_t wyaw_</div><div class="ttdoc">weight of yaw </div><div class="ttdef"><b>Definition:</b> env_base.h:372</div></div>
<div class="ttc" id="structWaypoint_html_ad5c2f2b8f6dba96bb93faccde844e9a8"><div class="ttname"><a href="structWaypoint.html#ad5c2f2b8f6dba96bb93faccde844e9a8">Waypoint::yaw</a></div><div class="ttdeci">decimal_t yaw</div><div class="ttdoc">yaw </div><div class="ttdef"><b>Definition:</b> waypoint.h:36</div></div>
<div class="ttc" id="classMPL_1_1env__map_html_a27101292af5b56676fca7374e1c1f5f1"><div class="ttname"><a href="classMPL_1_1env__map.html#a27101292af5b56676fca7374e1c1f5f1">MPL::env_map::set_gradient_weight</a></div><div class="ttdeci">void set_gradient_weight(decimal_t w)</div><div class="ttdoc">Set gradient weight. </div><div class="ttdef"><b>Definition:</b> env_map.h:178</div></div>
<div class="ttc" id="classMPL_1_1env__map_html_affcb3f4534d828b9d2751292cef5cf24"><div class="ttname"><a href="classMPL_1_1env__map.html#affcb3f4534d828b9d2751292cef5cf24">MPL::env_map::traverse_trajectory</a></div><div class="ttdeci">decimal_t traverse_trajectory(const Trajectory&lt; Dim &gt; &amp;traj) const</div><div class="ttdoc">traverse trajectory </div><div class="ttdef"><b>Definition:</b> env_map.h:229</div></div>
<div class="ttc" id="classMPL_1_1env__map_html_ae596cf73659ff74a03723acba510616d"><div class="ttname"><a href="classMPL_1_1env__map.html#ae596cf73659ff74a03723acba510616d">MPL::env_map::potential_map_</a></div><div class="ttdeci">std::vector&lt; int8_t &gt; potential_map_</div><div class="ttdoc">Potential map, optional. </div><div class="ttdef"><b>Definition:</b> env_map.h:290</div></div>
<div class="ttc" id="structWaypoint_html"><div class="ttname"><a href="structWaypoint.html">Waypoint</a></div><div class="ttdoc">Waypoint base class. </div><div class="ttdef"><b>Definition:</b> waypoint.h:23</div></div>
<div class="ttc" id="classMPL_1_1env__map_html_a3475ee37b51c1289a49726794b5b4451"><div class="ttname"><a href="classMPL_1_1env__map.html#a3475ee37b51c1289a49726794b5b4451">MPL::env_map::potential_weight_</a></div><div class="ttdeci">decimal_t potential_weight_</div><div class="ttdoc">Weight of potential value. </div><div class="ttdef"><b>Definition:</b> env_map.h:294</div></div>
<div class="ttc" id="classTrajectory_html_a93f4d5f86feb5f56e5fae7fd02311e9c"><div class="ttname"><a href="classTrajectory.html#a93f4d5f86feb5f56e5fae7fd02311e9c">Trajectory::getTotalTime</a></div><div class="ttdeci">decimal_t getTotalTime() const</div><div class="ttdoc">Get the total duration of Trajectory. </div><div class="ttdef"><b>Definition:</b> trajectory.h:300</div></div>
<div class="ttc" id="classMPL_1_1env__map_html_a4d832ec079d5ffe034aca016a9fa6227"><div class="ttname"><a href="classMPL_1_1env__map.html#a4d832ec079d5ffe034aca016a9fa6227">MPL::env_map::set_potential_weight</a></div><div class="ttdeci">void set_potential_weight(decimal_t w)</div><div class="ttdoc">Set potential weight. </div><div class="ttdef"><b>Definition:</b> env_map.h:186</div></div>
<div class="ttc" id="env__base_8h_html"><div class="ttname"><a href="env__base_8h.html">env_base.h</a></div><div class="ttdoc">environment base class </div></div>
<div class="ttc" id="classMPL_1_1env__base_html_a632147ec3e706f2bf10fb196c9d0a38c"><div class="ttname"><a href="classMPL_1_1env__base.html#a632147ec3e706f2bf10fb196c9d0a38c">MPL::env_base::U_</a></div><div class="ttdeci">vec_E&lt; VecDf &gt; U_</div><div class="ttdoc">Array of constant control input. </div><div class="ttdef"><b>Definition:</b> env_base.h:394</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_ac49a422f3e1b826123a12389024b9c50"><div class="ttname"><a href="classMPL_1_1env__base.html#ac49a422f3e1b826123a12389024b9c50">MPL::env_base::tol_acc_</a></div><div class="ttdeci">decimal_t tol_acc_</div><div class="ttdoc">tolerance of acceleration for goal region, 0 means no tolerance </div><div class="ttdef"><b>Definition:</b> env_base.h:378</div></div>
<div class="ttc" id="classTrajectory_html"><div class="ttname"><a href="classTrajectory.html">Trajectory</a></div><div class="ttdoc">Trajectory class. </div><div class="ttdef"><b>Definition:</b> trajectory.h:43</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_aee3b399510cbd3dc9690d65f0c099673"><div class="ttname"><a href="classMPL_1_1env__base.html#aee3b399510cbd3dc9690d65f0c099673">MPL::env_base::tol_yaw_</a></div><div class="ttdeci">decimal_t tol_yaw_</div><div class="ttdoc">tolerance of yaw for goal region, 0 means no tolerance </div><div class="ttdef"><b>Definition:</b> env_base.h:380</div></div>
<div class="ttc" id="classMPL_1_1env__map_html_a37c314a52d6bba0fa9675099d40b322c"><div class="ttname"><a href="classMPL_1_1env__map.html#a37c314a52d6bba0fa9675099d40b322c">MPL::env_map::set_gradient_map</a></div><div class="ttdeci">void set_gradient_map(const vec_E&lt; Vecf&lt; Dim &gt;&gt; &amp;map)</div><div class="ttdoc">Set gradient map. </div><div class="ttdef"><b>Definition:</b> env_map.h:175</div></div>
<div class="ttc" id="classMPL_1_1env__map_html_a40c301e16f4413100e8600b3931be7ad"><div class="ttname"><a href="classMPL_1_1env__map.html#a40c301e16f4413100e8600b3931be7ad">MPL::env_map::is_goal</a></div><div class="ttdeci">bool is_goal(const Waypoint&lt; Dim &gt; &amp;state) const</div><div class="ttdoc">Check if state hit the goal region, use L-1 norm. </div><div class="ttdef"><b>Definition:</b> env_map.h:25</div></div>
<div class="ttc" id="classMPL_1_1env__map_html_a1255eaee11eb80ce0e7f3a53f1f9b94e"><div class="ttname"><a href="classMPL_1_1env__map.html#a1255eaee11eb80ce0e7f3a53f1f9b94e">MPL::env_map::gradient_map_</a></div><div class="ttdeci">vec_E&lt; Vecf&lt; Dim &gt; &gt; gradient_map_</div><div class="ttdoc">Gradient map, optional. </div><div class="ttdef"><b>Definition:</b> env_map.h:292</div></div>
<div class="ttc" id="classMPL_1_1env__map_html_a84ab1c20552e01476e40cc5f116d4d73"><div class="ttname"><a href="classMPL_1_1env__map.html#a84ab1c20552e01476e40cc5f116d4d73">MPL::env_map::traverse_primitive</a></div><div class="ttdeci">decimal_t traverse_primitive(const Primitive&lt; Dim &gt; &amp;pr) const</div><div class="ttdoc">Accumulate the cost along the primitive. </div><div class="ttdef"><b>Definition:</b> env_map.h:90</div></div>
<div class="ttc" id="map__util_8h_html"><div class="ttname"><a href="map__util_8h.html">map_util.h</a></div><div class="ttdoc">MapUtil classes. </div></div>
<div class="ttc" id="classPrimitive_html_a2654dd3b648c5e3ee7839344a8d9e730"><div class="ttname"><a href="classPrimitive.html#a2654dd3b648c5e3ee7839344a8d9e730">Primitive::evaluate</a></div><div class="ttdeci">Waypoint&lt; Dim &gt; evaluate(decimal_t t) const</div><div class="ttdoc">Return Waypoint at time . </div><div class="ttdef"><b>Definition:</b> primitive.h:321</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_a6d1787397e20de82495fb9c381c14980"><div class="ttname"><a href="classMPL_1_1env__base.html#a6d1787397e20de82495fb9c381c14980">MPL::env_base::yaw_max_</a></div><div class="ttdeci">decimal_t yaw_max_</div><div class="ttdoc">max yaw </div><div class="ttdef"><b>Definition:</b> env_base.h:388</div></div>
<div class="ttc" id="structWaypoint_html_a648217ed387d4e226bec8ecb05bfefe8"><div class="ttname"><a href="structWaypoint.html#a648217ed387d4e226bec8ecb05bfefe8">Waypoint::acc</a></div><div class="ttdeci">Vecf&lt; Dim &gt; acc</div><div class="ttdoc">acceleration in  </div><div class="ttdef"><b>Definition:</b> waypoint.h:34</div></div>
<div class="ttc" id="data__type_8h_html_a6b9210d48a0a541940af71827aad1b41"><div class="ttname"><a href="data__type_8h.html#a6b9210d48a0a541940af71827aad1b41">vec_E</a></div><div class="ttdeci">std::vector&lt; T, Eigen::aligned_allocator&lt; T &gt; &gt; vec_E</div><div class="ttdoc">Pre-allocated std::vector for Eigen using vec_E. </div><div class="ttdef"><b>Definition:</b> data_type.h:53</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_adcd1fa1f64b3b836228d9a79f91dff0e"><div class="ttname"><a href="classMPL_1_1env__base.html#adcd1fa1f64b3b836228d9a79f91dff0e">MPL::env_base::v_max_</a></div><div class="ttdeci">decimal_t v_max_</div><div class="ttdoc">max velocity </div><div class="ttdef"><b>Definition:</b> env_base.h:382</div></div>
<div class="ttc" id="structWaypoint_html_a91393e0d8d73afb9df512710e56f02a5"><div class="ttname"><a href="structWaypoint.html#a91393e0d8d73afb9df512710e56f02a5">Waypoint::vel</a></div><div class="ttdeci">Vecf&lt; Dim &gt; vel</div><div class="ttdoc">velocity in  </div><div class="ttdef"><b>Definition:</b> waypoint.h:33</div></div>
<div class="ttc" id="classMPL_1_1env__map_html_ad508bed13e2c697cb14a597615424d9f"><div class="ttname"><a href="classMPL_1_1env__map.html#ad508bed13e2c697cb14a597615424d9f">MPL::env_map::get_succ</a></div><div class="ttdeci">void get_succ(const Waypoint&lt; Dim &gt; &amp;curr, vec_E&lt; Waypoint&lt; Dim &gt;&gt; &amp;succ, std::vector&lt; decimal_t &gt; &amp;succ_cost, std::vector&lt; int &gt; &amp;action_idx) const</div><div class="ttdoc">Get successor. </div><div class="ttdef"><b>Definition:</b> env_map.h:147</div></div>
<div class="ttc" id="classMPL_1_1env__map_html_a5e41e4f10d5970bd03d512c7becce099"><div class="ttname"><a href="classMPL_1_1env__map.html#a5e41e4f10d5970bd03d512c7becce099">MPL::env_map::gradient_weight_</a></div><div class="ttdeci">decimal_t gradient_weight_</div><div class="ttdoc">Weight of gradient value. </div><div class="ttdef"><b>Definition:</b> env_map.h:296</div></div>
<div class="ttc" id="structWaypoint_html_a9661067e17763fffb65d99cabaff802a"><div class="ttname"><a href="structWaypoint.html#a9661067e17763fffb65d99cabaff802a">Waypoint::t</a></div><div class="ttdeci">decimal_t t</div><div class="ttdoc">time when reaching this waypoint in graph search </div><div class="ttdef"><b>Definition:</b> waypoint.h:37</div></div>
<div class="ttc" id="classPrimitive_html_a908ba5793fc0520602352b59358a34ea"><div class="ttname"><a href="classPrimitive.html#a908ba5793fc0520602352b59358a34ea">Primitive::t</a></div><div class="ttdeci">decimal_t t() const</div><div class="ttdoc">Return duration . </div><div class="ttdef"><b>Definition:</b> primitive.h:336</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_ad592f0117a0ef0ac15225855ae62c4cc"><div class="ttname"><a href="classMPL_1_1env__base.html#ad592f0117a0ef0ac15225855ae62c4cc">MPL::env_base::t_max_</a></div><div class="ttdeci">decimal_t t_max_</div><div class="ttdoc">max time </div><div class="ttdef"><b>Definition:</b> env_base.h:390</div></div>
<div class="ttc" id="data__type_8h_html_ac208833a6aa7ec29f39eb5ae142aaede"><div class="ttname"><a href="data__type_8h.html#ac208833a6aa7ec29f39eb5ae142aaede">Veci</a></div><div class="ttdeci">Eigen::Matrix&lt; int, N, 1 &gt; Veci</div><div class="ttdoc">Eigen 1D int vector of size N. </div><div class="ttdef"><b>Definition:</b> data_type.h:59</div></div>
<div class="ttc" id="classMPL_1_1env__map_html_ae21503c9a6461602c4e7e09db66b8111"><div class="ttname"><a href="classMPL_1_1env__map.html#ae21503c9a6461602c4e7e09db66b8111">MPL::env_map::info</a></div><div class="ttdeci">void info()</div><div class="ttdoc">Print out params. </div><div class="ttdef"><b>Definition:</b> env_map.h:258</div></div>
<div class="ttc" id="data__type_8h_html_a7c99d9360fc6cac2762b786e2fb52266"><div class="ttname"><a href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a></div><div class="ttdeci">double decimal_t</div><div class="ttdoc">Rename the float type used in lib. </div><div class="ttdef"><b>Definition:</b> data_type.h:49</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_a75662e4867f05ecc21cd1232d2126a11"><div class="ttname"><a href="classMPL_1_1env__base.html#a75662e4867f05ecc21cd1232d2126a11">MPL::env_base::dt_</a></div><div class="ttdeci">decimal_t dt_</div><div class="ttdoc">duration of primitive </div><div class="ttdef"><b>Definition:</b> env_base.h:392</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_a6a67a6409ed46cd0c04f095a18996178"><div class="ttname"><a href="classMPL_1_1env__base.html#a6a67a6409ed46cd0c04f095a18996178">MPL::env_base::j_max_</a></div><div class="ttdeci">decimal_t j_max_</div><div class="ttdoc">max jerk </div><div class="ttdef"><b>Definition:</b> env_base.h:386</div></div>
<div class="ttc" id="classMPL_1_1env__map_html_a2d7be70921dce29559e33f3e59fc8c5a"><div class="ttname"><a href="classMPL_1_1env__map.html#a2d7be70921dce29559e33f3e59fc8c5a">MPL::env_map::set_prior_trajectory</a></div><div class="ttdeci">void set_prior_trajectory(const Trajectory&lt; Dim &gt; &amp;traj)</div><div class="ttdoc">Set prior trajectory. </div><div class="ttdef"><b>Definition:</b> env_map.h:189</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_a407c61fa8bfdefc3012546f0c412acd3"><div class="ttname"><a href="classMPL_1_1env__base.html#a407c61fa8bfdefc3012546f0c412acd3">MPL::env_base::expanded_nodes_</a></div><div class="ttdeci">vec_Vecf&lt; Dim &gt; expanded_nodes_</div><div class="ttdoc">expanded nodes for debug </div><div class="ttdef"><b>Definition:</b> env_base.h:402</div></div>
<div class="ttc" id="classTrajectory_html_a25ec384d286281c81480dee1b2f7bffe"><div class="ttname"><a href="classTrajectory.html#a25ec384d286281c81480dee1b2f7bffe">Trajectory::sample</a></div><div class="ttdeci">vec_E&lt; Command&lt; Dim &gt; &gt; sample(int N) const</div><div class="ttdoc">Sample N+1 Command along trajectory using uniformed time. </div><div class="ttdef"><b>Definition:</b> trajectory.h:230</div></div>
<div class="ttc" id="classMPL_1_1env__map_html_a7229298e89baa8b1acb1ed1affda30a4"><div class="ttname"><a href="classMPL_1_1env__map.html#a7229298e89baa8b1acb1ed1affda30a4">MPL::env_map::is_free</a></div><div class="ttdeci">bool is_free(const Primitive&lt; Dim &gt; &amp;pr) const</div><div class="ttdoc">Check if the primitive is in free space. </div><div class="ttdef"><b>Definition:</b> env_map.h:60</div></div>
<div class="ttc" id="data__type_8h_html_a135c596de9b80bec15985876ebd5036e"><div class="ttname"><a href="data__type_8h.html#a135c596de9b80bec15985876ebd5036e">Vec2f</a></div><div class="ttdeci">Vecf&lt; 2 &gt; Vec2f</div><div class="ttdoc">Eigen 1D float vector of size 2. </div><div class="ttdef"><b>Definition:</b> data_type.h:76</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_a8e6706c18fa58da08174f7c0c8209312"><div class="ttname"><a href="classMPL_1_1env__base.html#a8e6706c18fa58da08174f7c0c8209312">MPL::env_base::goal_node_</a></div><div class="ttdeci">Waypoint&lt; Dim &gt; goal_node_</div><div class="ttdoc">Goal node. </div><div class="ttdef"><b>Definition:</b> env_base.h:396</div></div>
<div class="ttc" id="classPrimitive_html_a6c369ebe499544a832c88b6fc4ada0e4"><div class="ttname"><a href="classPrimitive.html#a6c369ebe499544a832c88b6fc4ada0e4">Primitive::max_vel</a></div><div class="ttdeci">decimal_t max_vel(int k) const</div><div class="ttdoc">Return max velocity along one axis. </div><div class="ttdef"><b>Definition:</b> primitive.h:353</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_a7be6859be90f54ef175482d0aa6020c9"><div class="ttname"><a href="classMPL_1_1env__base.html#a7be6859be90f54ef175482d0aa6020c9">MPL::env_base::a_max_</a></div><div class="ttdeci">decimal_t a_max_</div><div class="ttdoc">max acceleration </div><div class="ttdef"><b>Definition:</b> env_base.h:384</div></div>
<div class="ttc" id="primitive_8h_html_a4e7cc1831110e0cd23fa969236c8b9f5"><div class="ttname"><a href="primitive_8h.html#a4e7cc1831110e0cd23fa969236c8b9f5">validate_primitive</a></div><div class="ttdeci">bool validate_primitive(const Primitive&lt; Dim &gt; &amp;pr, decimal_t mv=0, decimal_t ma=0, decimal_t mj=0, decimal_t myaw=0)</div><div class="ttdoc">Check if the max velocity magnitude is within the threshold. </div><div class="ttdef"><b>Definition:</b> primitive.h:450</div></div>
<div class="ttc" id="data__type_8h_html_a3a0c45655a5e009e56634ccde0c5c575"><div class="ttname"><a href="data__type_8h.html#a3a0c45655a5e009e56634ccde0c5c575">Vecf</a></div><div class="ttdeci">Eigen::Matrix&lt; decimal_t, N, 1 &gt; Vecf</div><div class="ttdoc">Eigen 1D float vector of size N. </div><div class="ttdef"><b>Definition:</b> data_type.h:56</div></div>
<div class="ttc" id="classMPL_1_1env__base_html"><div class="ttname"><a href="classMPL_1_1env__base.html">MPL::env_base</a></div><div class="ttdoc">Base environment class. </div><div class="ttdef"><b>Definition:</b> env_base.h:17</div></div>
<div class="ttc" id="classMPL_1_1env__map_html_a0d59fd81da8205001281395b80a4c6fa"><div class="ttname"><a href="classMPL_1_1env__map.html#a0d59fd81da8205001281395b80a4c6fa">MPL::env_map::map_util_</a></div><div class="ttdeci">std::shared_ptr&lt; MapUtil&lt; Dim &gt; &gt; map_util_</div><div class="ttdoc">Collision checking util. </div><div class="ttdef"><b>Definition:</b> env_map.h:288</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_a5c6a2e729131e36e72b51266d887ae8f"><div class="ttname"><a href="classMPL_1_1env__base.html#a5c6a2e729131e36e72b51266d887ae8f">MPL::env_base::w_</a></div><div class="ttdeci">decimal_t w_</div><div class="ttdoc">weight of time cost </div><div class="ttdef"><b>Definition:</b> env_base.h:370</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_a12ccadddac2cf7cf643ce4af8982054c"><div class="ttname"><a href="classMPL_1_1env__base.html#a12ccadddac2cf7cf643ce4af8982054c">MPL::env_base::heur_ignore_dynamics_</a></div><div class="ttdeci">bool heur_ignore_dynamics_</div><div class="ttdoc">if enabled, ignore dynamics when calculate heuristic </div><div class="ttdef"><b>Definition:</b> env_base.h:368</div></div>
<div class="ttc" id="classPrimitive_html_ad46e6378e08cd0f2d829787d8c44b93d"><div class="ttname"><a href="classPrimitive.html#ad46e6378e08cd0f2d829787d8c44b93d">Primitive::sample</a></div><div class="ttdeci">vec_E&lt; Waypoint&lt; Dim &gt; &gt; sample(int N) const</div><div class="ttdoc">Sample N+1 Waypoints using uniformed time. </div><div class="ttdef"><b>Definition:</b> primitive.h:415</div></div>
<div class="ttc" id="classMPL_1_1env__map_html"><div class="ttname"><a href="classMPL_1_1env__map.html">MPL::env_map</a></div><div class="ttdoc">Voxel map environment. </div><div class="ttdef"><b>Definition:</b> env_map.h:19</div></div>
<div class="ttc" id="classMPL_1_1env__map_html_a1c34f1f4e9c822470d3a15c88f3164fc"><div class="ttname"><a href="classMPL_1_1env__map.html#a1c34f1f4e9c822470d3a15c88f3164fc">MPL::env_map::env_map</a></div><div class="ttdeci">env_map(std::shared_ptr&lt; MapUtil&lt; Dim &gt;&gt; map_util)</div><div class="ttdoc">Constructor with map util as input. </div><div class="ttdef"><b>Definition:</b> env_map.h:22</div></div>
<div class="ttc" id="classPrimitive_html"><div class="ttname"><a href="classPrimitive.html">Primitive</a></div><div class="ttdoc">Primitive class. </div><div class="ttdef"><b>Definition:</b> primitive.h:206</div></div>
<div class="ttc" id="classMPL_1_1MapUtil_html"><div class="ttname"><a href="classMPL_1_1MapUtil.html">MPL::MapUtil</a></div><div class="ttdef"><b>Definition:</b> map_util.h:20</div></div>
<div class="ttc" id="structWaypoint_html_af63de420a521f105eea4ce098a51ad7e"><div class="ttname"><a href="structWaypoint.html#af63de420a521f105eea4ce098a51ad7e">Waypoint::pos</a></div><div class="ttdeci">Vecf&lt; Dim &gt; pos</div><div class="ttdoc">position in  </div><div class="ttdef"><b>Definition:</b> waypoint.h:32</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_ac85f1da0ec8736ba5c379bbdb6634fe2"><div class="ttname"><a href="classMPL_1_1env__base.html#ac85f1da0ec8736ba5c379bbdb6634fe2">MPL::env_base::tol_pos_</a></div><div class="ttdeci">decimal_t tol_pos_</div><div class="ttdoc">tolerance of position for goal region, 0.5 is the default </div><div class="ttdef"><b>Definition:</b> env_base.h:374</div></div>
<div class="ttc" id="classMPL_1_1env__map_html_a257df36534163eef0292854d38477a6b"><div class="ttname"><a href="classMPL_1_1env__map.html#a257df36534163eef0292854d38477a6b">MPL::env_map::is_free</a></div><div class="ttdeci">bool is_free(const Vecf&lt; Dim &gt; &amp;pt) const</div><div class="ttdoc">Check if a point is in free space. </div><div class="ttdef"><b>Definition:</b> env_map.h:48</div></div>
<div class="ttc" id="classMPL_1_1env__map_html_a061bd8a136a679ee196455c8170269bc"><div class="ttname"><a href="classMPL_1_1env__map.html#a061bd8a136a679ee196455c8170269bc">MPL::env_map::set_potential_map</a></div><div class="ttdeci">void set_potential_map(const std::vector&lt; int8_t &gt; &amp;map)</div><div class="ttdoc">Set potential map. </div><div class="ttdef"><b>Definition:</b> env_map.h:181</div></div>
</div><!-- fragment --></div><!-- contents -->
<!-- start footer part -->
<hr class="footer"/><address class="footer"><small>
Generated by &#160;<a href="http://www.doxygen.org/index.html">
<img class="footer" src="doxygen.png" alt="doxygen"/>
</a> 1.8.13
</small></address>
</body>
</html>
